package NXTExplorer;

import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.localization.OdometryPoseProvider;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;
import sharedData.SharedConstants;
import Behaviors.Bump;
import Behaviors.Detect;
import Behaviors.Exit;
import Behaviors.Forward;
import data.IDataQueue;

/**
 * Thread that uses subsumption architecture.
 * 
 * @author Didzis Petkus, Danny Johansen, Liviu Berheci
 * 
 */
public class ExplorerBot extends Thread {

	private IDataQueue queue;
	private SensorParameter params;
	public static int USSRMode;
	public static int lastDistanceKnown;

	/**
	 * Default constructor.
	 * 
	 * @param q
	 *            - the given IDataQueue object
	 */
	public ExplorerBot(IDataQueue q) {
		this.queue = q;
		USSRMode = 0;
	}

	/**
	 * Starts the thread. Sets up and starts the subsumption architecture.
	 */
	@Override
	public void run() {

		// Create and setup pilot
		DifferentialPilot pilot = new DifferentialPilot(
				DifferentialPilot.WHEEL_SIZE_NXT2,
				SharedConstants.PILOT_TRACKWIDTH, Motor.A, Motor.C);
		pilot.setTravelSpeed(SharedConstants.PILOT_TRAVELSPEED);
		pilot.setRotateSpeed(30);

		// port number must be changed as needed , front
		UltrasonicSensor centerUssr = new UltrasonicSensor(SensorPort.S3);

		// port number must be changed as needed , left
		UltrasonicSensor leftUssr = new UltrasonicSensor(SensorPort.S1);

		// port number must be changed as needed , right
		UltrasonicSensor rightUssr = new UltrasonicSensor(SensorPort.S4);

		TouchSensor ts = new TouchSensor(SensorPort.S2);

		OdometryPoseProvider poseProvider = new OdometryPoseProvider(pilot);

		params = new SensorParameter(pilot, poseProvider, queue, ts, leftUssr,
				rightUssr, centerUssr);

		Behavior b1 = new Exit();
		Behavior b2 = new Bump(params);
		// Behavior b3 = new ArcSearch(params);
		Behavior b4 = new Detect(params);
		// Behavior b5 = new FollowWall(params);
		Behavior b6 = new Forward(params);

		Behavior[] behaviors = { b6, b4, b2, b1 };

		Arbitrator arby = new Arbitrator(behaviors);
		arby.start();

	}
}
